#include "algorithm.h"
#include <QDebug>







void frequencySpectrum(const QVector<double> &src, QVector<double> &values)
{
    int len = src.size();
    if(len <= 0)
        return;

    int flen = 1;
    while (true)
    {
        if(flen > len)
            break;
        else
            flen *= 2;
    }
    flen /= 2;
    QVector<std::complex<double>> in(flen), out(flen);
    for(int i = 0; i < flen; i++)
        in[i] = std::complex<double>(src.at(i), 0);
    FourierTransform(in, out);

    int length = flen / 2 + 1;
    len = out.size();
    for(int i = 0; i < length; i++)
    {
        double v = out.at(i).real() / length * 2.;
        values.append(v);
    }
}





Algorithm::Algorithm(QObject *parent)
    : QObject(parent)
{
}

void Algorithm::FFT(const QVector<std::complex<double>> &in, QVector<std::complex<double>> &out)
{
    int n = in.size();		//输入的点数，n>0
    int it, k(0);

    for(it = 1; ; it *= 2)	//求k值, 要满足n等于2的k次方
    {
        if(it == n)
            break;
        k++;
        if(k > 64)
            return;
    }

    out.resize(n);
    QVector<std::complex<double>> temp(n);
    for(it = 0; it < n; it++) {
        int m = it, is(0);
        for(int i = 0; i < k; i++) {
            int j = m / 2;
            is = 2 * is + (m - 2 * j);
            m = j;
        }
        temp[it] = in[is];
    }
    out[0] = std::complex<double>(1.0, 0.0);
    double p = (M_PI * 2) / (1.0 * n);
    out[1] = std::complex<double>(qCos(p), -qSin(p));
    out[1] = std::complex<double>(in[1].real(), -in[1].imag());
    for(int i = 2; i < n; i ++) {
        p = in[i-1].real() * in[1].real();

        double q = in[i-1].imag() * in[1].imag();
        double s = (in[i-1].real() + in[i-1].imag()) * (in[1].real() + in[1].imag());

        out[i] = std::complex<double>(p - q, s - p - q);
    }
    for(it = 0; it<n-1; it = it + 2) {
        double vr = temp[it].real();
        double vi = temp[it].imag();

        temp[it]    = std::complex<double>(vr + temp[it+1].real(), vi + temp[it+1].imag());
        temp[it+1]  = std::complex<double>(vr - temp[it+1].real(), vi - temp[it+1].imag());
    }

    int m=n/2;
    int nv(2);

    for(int l0 = k-2; l0 >= 0; l0--) {
        m = m / 2;
        nv = 2 * nv;
        for(it = 0; it <= (m - 1) * nv; it = it + nv)
          for(int j = 0; j < (nv / 2); j ++)
          {
              p = in[m*j].real() * temp[it+j+nv/2].real();
              double q = in[m*j].imag() * temp[it+j+nv/2].imag();
              double s = in[m*j].real() + in[m*j].imag();

              s *= (temp[it+j+nv/2].real() + temp[it+j+nv/2].imag());
              double poddr = p - q;
              double poddi = s - p - q;

              temp[it+j+nv/2] = std::complex<double>(temp[it+j].real() - poddr, temp[it+j].imag() - poddi);
              temp[it+j] = std::complex<double>(temp[it+j].real() + poddr, temp[it+j].imag() + poddi);
          }
    }

    for(int i = 0; i < n; i ++) {
        temp[i] = std::complex<double>(temp[i].real() / (1.0*n), temp[i].imag() / (1.0*n));
    }
    for(int i=0; i<n; i++)
    {
        out[i] = std::complex<double>(sqrt(temp[i].real() * temp[i].real() + temp[i].imag() * temp[i].imag()),
                                   in[i].imag());
        if(qAbs(temp[i].real()) < 0.000001 * qAbs(temp[i].imag())) {
            if((temp[i].imag() * temp[i].real()) > 0)
                out[i] = std::complex<double>(in[i].real(), 90.0);
            else
                out[i] = std::complex<double>(in[i].real(), -90.0);
        }
        else
            out[i] = std::complex<double>(in[i].real(), (qAtan(temp[i].imag() / temp[i].real()) * 360.0 / 6.283185306));
    }
}

int Algorithm::powerValue(int len)
{
    if(len < 2)
        return 0;
    int n = 1;
    while (n <= len)
    {
        n *= 2;
    }
    n /= 2;
    return n;
}


int Algorithm::get_computation_layers(int num)
{
    int nLayers = 0;
    int len = num;
    if (len == 2)
        return 1;
    while (true)
    {
        len = len / 2;
        nLayers++;
        if (len == 2)
            return nLayers + 1;
        if (len < 1)
            return -1;
    }
}











Filter::Filter(FilterType fType, FilterWindowType wType, int scala, qreal freq, qreal freqLower, qreal freqUpper) :
    lower(0),
    upper(0),
    Type(fType),
    WinType(wType),
    Scala(scala),
    Freq(freq),
    lFreq(freqLower),
    hFreq(freqUpper)
{
    ImpactData.resize(Scala + 1);

    calcImpactData();
}

Filter::~Filter()
{
}

void Filter::calcImpactData()
{
    qreal Beta = 0.0;
    if(KAISER == WinType)
        Beta = 7.865;
    int Mid = 0, Len = 0;
    if(Scala % 2 == 0) {
        Len = Scala / 2 - 1;
        Mid = 1;
    }
    else {
        Len = Scala / 2;
        Mid = 0;
    }
    qreal Delay = Scala / 2.0;
    qreal AngFreq = 2.0 * M_PI * (lFreq / Freq);
    qreal hAngFreq = 0;
    if(BANDPASS == Type || BANDSTOP == Type)
        hAngFreq = 2.0 * M_PI * (hFreq / Freq);
    switch (Type) {
    case LOWPASS:
    {
        for (auto i = 0; i <= Len; i++) {
            qreal s = i - Delay;
            qreal wVal = calcByWindowType(WinType, Scala + 1, i, Beta);
            qreal val = (sin(AngFreq * s) / (M_PI * s)) * wVal;
            ImpactData[i] = val;
            ImpactData[Scala - i] = ImpactData[i];
        }
        if(1 == Mid) {
            ImpactData[Scala / 2] = AngFreq / M_PI;
        }
        break;
    }
    case HIGHPASS:
    {
        for (auto i = 0; i <= Len; i++) {
            qreal s = i - Delay;
            qreal wVal = calcByWindowType(WinType, Scala + 1, i, Beta);
            ImpactData[i] = (sin(M_PI * s) - sin(AngFreq * s)) / (M_PI * s) * wVal;
            ImpactData[Scala - i] = ImpactData[i];
        }
        if(1 == Mid)
            ImpactData[Scala / 2] = 1.0 - AngFreq / M_PI;
        break;
    }
    case BANDPASS:
    {
        for (auto i = 0; i <= Len; i++) {
            qreal s = i - Delay;
            qreal wVal = calcByWindowType(WinType, Scala + 1, i, Beta);
            ImpactData[i] = (sin(hAngFreq * s) - sin(AngFreq * s)) / (M_PI * s) * wVal;
            ImpactData[Scala - i] = ImpactData[i];
        }
        if(1 == Mid)
            ImpactData[Scala / 2] = (hAngFreq - AngFreq) / M_PI;
        break;
    }
    case BANDSTOP:
    {
        for (auto i = 0; i <= Len; i++) {
            qreal s = i - Delay;
            qreal wVal = calcByWindowType(WinType, Scala + 1, i, Beta);
            ImpactData[i] = (sin(AngFreq * s) + sin(M_PI * s) - sin(hAngFreq * s)) / (M_PI * s) * wVal;
            ImpactData[Scala - i] = ImpactData[i];
        }
        if(1 == Mid)
            ImpactData[Scala / 2] = (AngFreq + M_PI - hAngFreq) / M_PI;
        break;
    }
    default:
        break;
    }
}

void Filter::setFilterType(FilterType fType)
{
    if(Type == fType)
        return;
    Type = fType;

    calcImpactData();
}

void Filter::setFilterWinType(FilterWindowType wType)
{
    if(WinType == wType)
        return;
    WinType = wType;

    calcImpactData();
}

void Filter::setWinLength(int len)
{
    if(Scala == len)
        return;
    Scala = len;

    ImpactData.resize(Scala + 1);
    calcImpactData();
}

void Filter::setFreqency(int freq)
{
    if(Freq == freq)
        return;
    Freq = freq;
    calcImpactData();
}

void Filter::setFreqUpper(int fUpper)
{
    if(hFreq == fUpper)
        return;
    hFreq = fUpper;
    calcImpactData();
}

void Filter::setFreqLower(int fLower)
{
    if(lFreq == fLower)
        return;
    lFreq = fLower;
    calcImpactData();
}

void Filter::setFilterParams(FilterType fType, FilterWindowType wType, int scale, qreal freq, qreal freqLower, qreal freqUpper)
{
    setFilterType(fType);
    setFilterWinType(wType);
    setWinLength(scale);
    setFreqency(freq);
    setFreqLower(freqLower);
    setFreqUpper(freqUpper);
}

void Filter::Convolution(const QVector<qreal> &d)
{
    RawData = d;
    int dataLen = d.size();
    OutData.resize(dataLen);
    for (auto i = 0; i < dataLen; i++)
    {
        OutData[i] = 0;
        qreal sum = 0;
        if(i < Scala)
        {
            for (auto j = 0; j <= i; j++)
            {
                sum = sum + RawData.at(j) * ImpactData.at(i - j);
            }
        }
        if(i >= Scala && i < dataLen)
        {
            int temp = Scala - 1;
            for (auto j = i - Scala; j < i; j++)
            {
                sum = sum + RawData.at(j) * ImpactData.at(temp);
                --temp;
            }
        }
        if(i > dataLen)
        {
            int temp = dataLen - 1;
            for (auto j = 0; j < Scala; j++)
            {
                sum = sum + RawData.at(temp) * ImpactData.at(j);
                --temp;
            }
            if(i >= dataLen - Scala)
                m_preFilter.append(RawData.at(i));
            if(m_preFilter.size() > Scala)
                m_preFilter.pop_front();
        }
        OutData[i] = sum;

        if(0 == i)
        {
            lower = upper = sum;
        }
        else
        {
            if(lower > sum)
                lower = sum;
            if(upper < sum)
                upper = sum;
        }
    }
}

qreal Filter::calcByWindowType(int WinType, int Scala, int Index, qreal Beta)
{
    qreal Value = 1.0;
    switch (WinType) {
    case RECTANGLE:
        Value = 1.0;
        break;
    case TUKEY:
    {
        int k = (Scala - 2) / 10;
        if(Index <= k)
            Value = 0.5 * (1.0 - cos(Index * M_PI / (k + 1)));
        if(Index > (Scala - k - 2))
            Value = 0.5 * (1.0 - cos((Scala - Index - 1) * M_PI / (k + 1)));
        break;
    }
    case TRIANGLE:
        Value = 1.0 - fabs(1.0 - 2 * Index / (Scala * 1.0));
        break;
    case HANN:
        Value = 0.5 * (1.0 - cos(2 * Index * M_PI / (Scala - 1)));
        break;
    case HANNING:
        Value = 0.54 - 0.36 * cos(2 * Index * M_PI / (Scala - 1));
        break;
    case BRACKMAN:
        Value = 0.42 * 0.5 * cos(2 * Index * M_PI / (Scala - 1)) + 0.08 * cos(4 * Index * M_PI / (Scala - 1));
        break;
    case KAISER:
        Value = calcKaisar(Scala, Index, Beta);
        break;
    default:
        break;
    }
    return Value;
}

qreal Filter::calcKaisar(int Scala, int Index, qreal Beta)
{
    qreal a = 2.0 * Index / (Scala - 1) - 1.0;
    return  bessel(Beta * sqrt(1.0 - a * a)) / bessel(Beta);
}

qreal Filter::bessel(qreal Beta)
{
    qreal d = 1.0, d2 = 0, sum = 1.0;
    for (auto i = 1; i <= 25; i++)
    {
        d = d * Beta / 2.0 / i;
        d2 = d * d;
        sum += d2;
        if(d2 < sum * (1.0e-8))
            break;
    }
    return sum;
}

